#include "frame.h"

#include <iostream>


#include <QDebug>

//This is for debug only
void print_matrix(glm::mat4 m){
    glm::vec4 c1;
    glm::vec4 c2;
    glm::vec4 c3;
    glm::vec4 c4;
    c1 = m[0];
    c2 = m[1];
    c3 = m[2];
    c4 = m[3];
    std::cout << c1.x << " " << c2.x << " " << c3.x << " " << c4.x << std::endl;
    std::cout << c1.y << " " << c2.y << " " << c3.y << " " << c4.y << std::endl;
    std::cout << c1.z << " " << c2.z << " " << c3.z << " " << c4.z << std::endl;
    std::cout << c1.w << " " << c2.w << " " << c3.w << " " << c4.w << std::endl;
}

Frame::Frame()
    :m_update(true),
      m_x_angle(0),
      m_y_angle(0),
      m_z_angle(0),
      m_rotate_axis(1.0, 0.0, 0.0),
      m_scale_factors(1.0, 1.0, 1.0, 0.0)
{
    //m_frame_matrix = glm::mat4(1.0);
    //m_scale_matrix = glm::mat4(1.0);
}

glm::mat4 Frame::world_to_local(){
    if ( m_update ) {
        // Recalculate
        return glm::inverse(local_to_world());
        //        local = glm::transpose(world);
        //        local[0][3] = -local[0][3];
        //        local[1][3] = -local[1][3];
        //        local[2][3] = -local[2][3];

        //        local[3][0] = local[3][1] = local[3][2] = 0.0f;
        //localScale = glm::scale(glm::mat4(), glm::vec3(1/scaleFactors.x, 1/scaleFactors.y, 1/scaleFactors.z));
    }
    return glm::inverse(local_to_world());
    //return localScale*local;

//   return  glm::inverse(m_frame_matrix) * glm::inverse(m_scale_matrix);
}

glm::mat4 Frame::local_to_world(){
    glm::vec4 right(1.0, 0.0, 0.0, 0.0);
    glm::vec4 up(0.0, 1.0, 0.0, 0.0);
    glm::vec4 look(0.0, 0.0, 1.0, 0.0);

    if ( m_update ) {
        // Recalculate
        m_to_world_matrix = glm::mat4(1.0);

        up = m_to_world_matrix*up;
        right = m_to_world_matrix*right;
        look = m_to_world_matrix*look;

//        m_to_world_matrix = glm::mat4();
//        m_to_world_matrix[0] = right;
//        m_to_world_matrix[1] = up;
//        m_to_world_matrix[2] = look;
//        m_to_world_matrix[3] = glm::vec4(glm::dot( m_origin, right ),
//                                         glm::dot( m_origin, up ),
//                                         glm::dot( m_origin, look ),
//                                         1.0f);
        m_to_world_matrix = glm::translate(m_to_world_matrix, glm::vec3(m_origin.x,m_origin.y,m_origin.z));

        m_to_world_matrix = glm::rotate(m_to_world_matrix, m_x_angle, glm::vec3(1.0,0.0,0.0));
        m_to_world_matrix = glm::rotate(m_to_world_matrix, m_y_angle, glm::vec3(0.0,1.0,0.0));
        m_to_world_matrix = glm::rotate(m_to_world_matrix, m_z_angle, glm::vec3(0.0,0.0,1.0));

        m_world_scale = glm::scale(glm::mat4(1.0), glm::vec3(m_scale_factors));
    }
//        qDebug() << "C0: " << m_to_world_matrix[0].x << " "  << m_to_world_matrix[0].y << " "  << m_to_world_matrix[0].z << " "  << m_to_world_matrix[0].w;
//        qDebug() << "C1: " << m_to_world_matrix[1].x << " "  << m_to_world_matrix[1].y << " "  << m_to_world_matrix[1].z << " "  << m_to_world_matrix[1].w;
//        qDebug() << "C3: " << m_to_world_matrix[2].x << " "  << m_to_world_matrix[2].y << " "  << m_to_world_matrix[2].z << " "  << m_to_world_matrix[2].w;
//        qDebug() << "C4: " << m_to_world_matrix[3].x << " "  << m_to_world_matrix[3].y << " "  << m_to_world_matrix[3].z << " "  << m_to_world_matrix[3].w;
    return m_to_world_matrix*m_world_scale;
//   return m_frame_matrix * m_scale_matrix;
}

void Frame::rotate(float angle, glm::vec3 axis){
    m_update = true;
    m_rotate_axis = axis;
    if(axis.x > 0){
        m_x_angle = angle;
        return;
    }
    if(axis.y > 0){
        m_y_angle = angle;
        return;
    }
    if(axis.z > 0){
        m_z_angle = angle;
        return;
    }
    //m_to_world_matrix = glm::rotate(m_to_world_matrix, angle, axis);
    //m_frame_matrix = glm::rotate(m_frame_matrix,angle,axis);
}

void Frame::translate(float x, float y, float z){
    m_update = true;
    m_origin = m_origin + glm::vec4(x, y, z, 0);
    //m_frame_matrix = glm::translate(m_frame_matrix,t);
}

void Frame::scale(float x, float y, float z){
    m_update = true;
    m_scale_factors = glm::vec4(x, y, z, 0);
//    m_scale_matrix = glm::scale(m_scale_matrix,s);
}


void Frame::load_identity(){
    m_update = true;
    m_to_world_matrix = m_to_local_matrix = m_world_scale = m_local_scale = glm::mat4(1.0);
//    m_frame_matrix = glm::mat4(1.0);
//    m_scale_matrix = glm::mat4(1.0);
}




